/*
 * Utility.cpp
 *
 *  Created on: 10/ott/2013
 *      Author: alessandrob
 */
#include<iostream>
#include<arpa/inet.h>
#include<unistd.h>
#include<sys/socket.h>
#include<sys/types.h>
#include<stdio.h>
#include<string.h>
#include<stdlib.h>
#include<opencv2/opencv.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include "Utility.h"
using namespace std;
using namespace cv;

void printErrorLog(string s)
{
	cout<<s<<endl;
	exit(1);
}
//Serialize float
void serializeKeypoints(vector<KeyPoint> kp, uchar* keypoints)
{
	unsigned int number;
	for(int i=0,k=0;i<kp.size();i++,k++)
	{
		number = *((unsigned int*)&kp[i].pt.x);
		keypoints[k] = (number >> 24);
		keypoints[k++] = (number >> 16);
		keypoints[k++] = (number >> 8);
		keypoints[k++] = (number);
		number = *((unsigned int*)&kp[i].pt.y);
		keypoints[k++] = (number >> 24);
		keypoints[k++] = (number >> 16);
		keypoints[k++] = (number >> 8);
		keypoints[k++] = (number);
	}
}
void deserializePackets(broadcast_packet packet[],int receivedBlocks,int blocks)
{
	int counter = 1;
	int k = 0;
	for(int i=0;i<blocks;i++)
	{
		packet[i].packetNumber = (packet[i].serializedPacket[4] << 24);
		packet[i].packetNumber = packet[i].packetNumber + (packet[i].serializedPacket[5] << 16);
		packet[i].packetNumber = packet[i].packetNumber + (packet[i].serializedPacket[6] << 8);
		packet[i].packetNumber = packet[i].packetNumber + (packet[i].serializedPacket[7]);
		if(packet[i].packetNumber != counter)
			packet[i].lost = true;
		else
			packet[i].lost = false;
		packet[i].payloadDimension = (packet[i].serializedPacket[0] << 24);
		packet[i].payloadDimension =  packet[i].payloadDimension + (packet[i].serializedPacket[1] << 16);
		packet[i].payloadDimension =  packet[i].payloadDimension + (packet[i].serializedPacket[2] << 8);
		packet[i].payloadDimension =  packet[i].payloadDimension + (packet[i].serializedPacket[3]);

	}
}
//Ordering packets by packet number
void orderPackets(broadcast_packet packet[],int blocks)
{
	broadcast_packet support;
	for(int i=0;i<blocks;i++)
	{
		for(int k=1;k<(blocks-i);k++)
		{
			if(packet[k].packetNumber>packet[k+1].packetNumber)
			{
				cout<<"Swapping"<<endl;
				memcpy(&support,&packet[k],sizeof(packet[k]));
				memcpy(&packet[k],&packet[k+1],sizeof(packet[k]));
				memcpy(&packet[k+1],&support,sizeof(packet[k]));
			}
		}
	}
}
//Building image from the packets received
Mat buildImage(broadcast_packet packet[],int blocks,int cols,int rows,int bytesPerPacket,int lastPacket)
{
	Mat picture;
	uchar data[cols*rows];
	int packetNumber = 1,offset=0;
	vector<int> params = vector<int>(2);
	params[0]=0;
	for(int i=0;packetNumber<(blocks+1);packetNumber++)
	{
		if(packet[i].packetNumber == packetNumber)
		{
			if(packetNumber == blocks && lastPacket>0)
			{
				//normal last packet
				for(int k=0;k<lastPacket;k++)
					data[offset+k]=packet[i].serializedPacket[k+8];
				offset = offset + lastPacket;
				i++;
			}
			else
			{
				//normal packet
				for(int k=0;k<bytesPerPacket;k++)
					data[offset+k]=packet[i].serializedPacket[k+8];
				offset = offset + bytesPerPacket;
				i++;
			}
		}
		else
		{
			//missing packet
			for(int k=0;k<bytesPerPacket;k++)
				data[offset+k]=255;
			offset = offset + bytesPerPacket;
		}
	}
	cout<<"Offset: "<<offset<<endl;
	picture = Mat(rows,cols,CV_8UC1,data,cols);
	//imwrite("/home/ubuntu/broadcastKeypoints.jpg",picture,params);
	return picture;
}

char* serializeCPUSpeed(int CPUspeed)
{
	char speed[4];
	speed[0]=CPUspeed >> 24;
	speed[1]=CPUspeed >> 16;
	speed[2]=CPUspeed >> 8;
	speed[3]=CPUspeed;
	return speed;
}

void sendData(int sockfd,double totaltime,int size,vector<KeyPoint> kp,Mat descriptors)
{
	int dataSent,i,cols,rows;
	int dataNumber = 1;
	for(i=0;i<dataNumber;i++)
	{
		char request;
		if(recv(sockfd,&request,sizeof(char),MSG_WAITALL)<0)
			printErrorLog("Request not received correctly");
		if(request=='t')
		{
			cout<<"Sending final processing time..."<<endl;
			dataSent=send(sockfd, &totaltime, sizeof(double), 0);
			if(dataSent<0)
				printErrorLog("Time sent not correctly");
			cout<<"Time sent correctly"<<endl;
		}
		/*
		else if(request=='k')
		{
			//Sending keypoints
			//cout<<"Sending keypoints detected..."<<endl;
			if ((dataSent = send(sockfd, &size, sizeof(int), 0))==-1)
				printErrorLog("Keypoints number not sent correctly");
			//cout<<"Keypoints number sent correctly: "<<dataSent<<endl;
			uchar keypoints[8*size];
			unsigned int number;
			float support;
			//Serialize keypoints
			for(int i=0,k=0;i<kp.size();i++,k=k+8)
			{
				support = kp[i].pt.x;
				//printf("x: %f ",support);
				number = *((unsigned int*)&support);
				keypoints[k] = (number >> 24);
				keypoints[k+1] = (number >> 16);
				keypoints[k+2] = (number >> 8);
				keypoints[k+3] = (number);
				support = kp[i].pt.y;
				//printf("x: %f ",support);
				number = *((unsigned int*)&support);
				keypoints[k+4] = (number >> 24);
				keypoints[k+5] = (number >> 16);
				keypoints[k+6] = (number >> 8);
				keypoints[k+7] = (number);
				//printf(" %d %d %d %d \n",keypoints[k],keypoints[k+1],keypoints[k+2],keypoints[k+3]);
				/*support = kp[i].pt.y;
				//printf("y: %f \n",support);
				number = *((unsigned int*)&support);
				keypoints[k+8] = (number >> 24);
				keypoints[k+9] = (number >> 16);
				keypoints[k+10] = (number >> 8);
				keypoints[k+11] = (number);
				support = kp[i].response;
				//printf("x: %f ",support);
				number = *((unsigned int*)&support);
				keypoints[k+12] = (number >> 24);
				keypoints[k+13] = (number >> 16);
				keypoints[k+14] = (number >> 8);
				keypoints[k+15] = (number);
				support = kp[i].size;
				//printf("x: %f ",support);
				number = *((unsigned int*)&support);
				keypoints[k+16] = (number >> 24);
				keypoints[k+17] = (number >> 16);
				keypoints[k+18] = (number >> 8);
				keypoints[k+19] = (number);
			}
			dataSent = send(sockfd, &keypoints, sizeof(char)*8*size, 0);
			if(dataSent!=(size*8*sizeof(char)))
				printErrorLog("Keypoints not sent correctly");
			cout<<"Keypoints sent correctly"<<endl;
		}
		else if(request=='d')
		{
			cout<<"Sending descriptors..."<<endl;
			//Sending descriptors
			cols = descriptors.cols;
			rows = descriptors.rows;
			//cout<<"Descriptors: rows "<<rows<<" cols "<<cols<<endl;
			int descriptorsSize = descriptors.rows*descriptors.cols;
			if (send(sockfd, &descriptorsSize, sizeof(int), 0)==-1)
				printErrorLog("Descriptors size sent not correctly");
			//packets = floor(descriptorsSize/BUFLEN);
			/*for(k=0,dataSent=0;k<(cols*rows);k=k+BUFLEN)
			{
				if(dataSent==(packets*BUFLEN) && (cols*rows)-(packets*BUFLEN)!=0)
				{
					dataSent = dataSent + sendto(sockfd,&descriptors.data[k],(cols*rows)-(packets*BUFLEN),0,(struct sockaddr*)&server_address,slen);
					cout<<dataSent<<endl;
				}
				else
					dataSent = dataSent + sendto(sockfd,&descriptors.data[k],BUFLEN,0,(struct sockaddr*)&server_address,slen);
			}
			uchar support[descriptorsSize];
			//memcpy(&support,&(descriptors.data[0]),descriptorsSize);
			dataSent = send(sockfd,&(descriptors.data[0]),descriptorsSize,0);
			if(dataSent != descriptorsSize)
				printErrorLog("Descriptors noy sent correctly");
			cout<<"Descriptors sent correctly"<<endl;
		}
	*/
	}
}

